/************************************************
本文件主要来自CSDN博主loop222对SimpleFOC项目的移植
原教程：https://blog.csdn.net/loop222/article/details/120471390
本项目删除了部分代码，将IIC改为了软件模拟IIC
************************************************/

#include "AS5600.h" 
#include "FOC.h" 
#include "LOWPASS_FILTER.h"
#include "gpio.h"
#include <math.h>

/******************************************************************************/
long cpr;
float full_rotation_offset;
long angle_data, angle_data_prev;
unsigned long velocity_calc_timestamp;
float angle_prev;

float angle_prev_position,full_rotation_offset_position;
/******************************************************************************/

#define IIC_SCL(x)        do{ x ? \
							  HAL_GPIO_WritePin(IIC_SCL_GPIO_Port, IIC_SCL_Pin, GPIO_PIN_SET) : \
							  HAL_GPIO_WritePin(IIC_SCL_GPIO_Port, IIC_SCL_Pin, GPIO_PIN_RESET); \
						  }while(0)       /* SCL */

#define IIC_SDA(x)        do{ x ? \
							  HAL_GPIO_WritePin(IIC_SDA_GPIO_Port, IIC_SDA_Pin, GPIO_PIN_SET) : \
							  HAL_GPIO_WritePin(IIC_SDA_GPIO_Port, IIC_SDA_Pin, GPIO_PIN_RESET); \
						  }while(0)       /* SDA */

#define IIC_READ_SDA     HAL_GPIO_ReadPin(IIC_SDA_GPIO_Port, IIC_SDA_Pin) /* ????SDA */

//2us
static void iic_delay(void)
{
	for(uint32_t j=0;j<10;j++);
}

void iic_start(void)
{
	IIC_SDA(1);
	IIC_SCL(1);
	iic_delay();
	IIC_SDA(0);
	iic_delay();
	IIC_SCL(0);
	iic_delay();
}

void iic_stop(void)
{
	IIC_SDA(0);     
	iic_delay();
	IIC_SCL(1);
	iic_delay();
	IIC_SDA(1);    
	iic_delay();
}

void iic_ack(void)
{
	IIC_SDA(0);     
	iic_delay();
	IIC_SCL(1);     
	iic_delay();
	IIC_SCL(0);
	iic_delay();
	IIC_SDA(1);     
	iic_delay();
}

void iic_nack(void)
{
	IIC_SDA(1);     
	iic_delay();
	IIC_SCL(1);     
	iic_delay();
	IIC_SCL(0);
	iic_delay();
}

uint8_t iic_wait_ack(void)
{
	uint8_t waittime = 0;
	uint8_t rack = 0;

	IIC_SDA(1);    
	iic_delay();
	IIC_SCL(1);    
	iic_delay();

	while (IIC_READ_SDA)   
	{
		waittime++;

		if (waittime > 250)
		{
			iic_stop();
			rack = 1;
			break;
		}
	}

	IIC_SCL(0);    
	iic_delay();
	return rack;
}

uint8_t iic_read_byte(uint8_t ack)
{
	uint8_t i, receive = 0;

	for (i = 0; i < 8; i++ )    
	{
		receive <<= 1;  
		IIC_SCL(1);
		iic_delay();

		if (IIC_READ_SDA)
		{
			receive++;
		}
		
		IIC_SCL(0);
		iic_delay();
	}

	if (!ack)
	{
		iic_nack();     
	}
	else
	{
		iic_ack();     
	}

	return receive;
}

void iic_send_byte(uint8_t data)
{
	uint8_t t;
	
	for (t = 0; t < 8; t++)
	{
		IIC_SDA((data & 0x80) >> 7);    
		iic_delay();
		IIC_SCL(1);
		iic_delay();
		IIC_SCL(0);
		data <<= 1;     
	}
	IIC_SDA(1);       
}

uint16_t AS5600_ReadTwoByte(uint16_t readAddr)
{
	uint16_t temp=0xFFFF;                                                                                   
	iic_start();  
	iic_send_byte((0X36<<1)|0x00);    
	iic_wait_ack(); 
	iic_send_byte(readAddr);   
	iic_wait_ack();        
	iic_start();              
	iic_send_byte((0X36<<1)|0x01);              
	iic_wait_ack();     
	temp=iic_read_byte(1);   
	temp=temp<<8|iic_read_byte(0); 
	iic_stop();     
	return temp;
}

/******************************************************************************/
#define  AS5600_Address  0x36
#define  RAW_Angle_Hi    0x0C
#define  AS5600_CPR      4096

unsigned short I2C_getRawCount()
{
	return AS5600_ReadTwoByte(0x0C);
}

/******************************************************************************/
void AS5600_Init(void)
{
	cpr=AS5600_CPR;
	angle_data = angle_data_prev = I2C_getRawCount();  
	full_rotation_offset = 0;
	velocity_calc_timestamp=0;
	full_rotation_offset_position=0;
}


#define _2PI 6.28318530718
/******************************************************************************/
float getAngle(void)
{
	float d_angle;
	
	angle_data = I2C_getRawCount();
	
	// tracking the number of rotations 
	// in order to expand angle range form [0,2PI] to basically infinity
	d_angle = angle_data - angle_data_prev;
	// if overflow happened track it as full rotation
	if(fabs(d_angle) > (0.8*cpr) ) full_rotation_offset_position += d_angle > 0 ? -_2PI : _2PI; 
	// save the current angle value for the next steps
	// in order to know if overflow happened
	angle_data_prev = angle_data;
	// return the full angle 
	// (number of full rotations)*2PI + current sensor angle15297823020
	
	return  (full_rotation_offset_position + ( angle_data * 1.0 / cpr) * _2PI) ;
}
/******************************************************************************/



/***********************************************
读取磁编码器360度值:(0°-360°)  (raw_data/2^12)*360°
***********************************************/
float get_Angle_360(void)
{
	return I2C_getRawCount()* 0.08789;
}

/***********************************************
磁编码器弧度制角度累计计算:(0-∞)
***********************************************/
float get_360Angle(void)
{
	float val = get_Angle_360();
	float d_angle = val - angle_prev_position;
	//计算旋转的圈数
	//通过判断角度变化是否大于80%的一圈(0.8f*6.28318530718f)来判断是否发生了溢出
	//如果发生了溢出,则将full_rotations增加1(如果d_angle小于0)或减少1(如果d_angle大于0)
	if( fabs(d_angle) > (0.8f*360) )
	full_rotation_offset_position += (d_angle > 0) ? -1 : 1;
	angle_prev_position = val;
	return (float)full_rotation_offset_position * 360 + angle_prev_position;
}

/***********************************************
读取磁编码器归一化弧度值:(0-6.28)		
***********************************************/
float get_Angle_2PI(void)
{
	return I2C_getRawCount()* 0.08789 / 57.32484;
}

/***********************************************
磁编码器弧度制角度累计计算:(0-∞)
***********************************************/
float get_Angle(void)
{
	float val = get_Angle_2PI();
	float d_angle = val - angle_prev;
	//计算旋转的圈数
	//通过判断角度变化是否大于80%的一圈(0.8f*6.28318530718f)来判断是否发生了溢出
	//如果发生了溢出,则将full_rotations增加1(如果d_angle小于0)或减少1(如果d_angle大于0)
	if( fabs(d_angle) > (0.8f*6.28318530718f) )
		full_rotation_offset += (d_angle > 0) ? -1 : 1;
	angle_prev = val;
	return (float)full_rotation_offset * _2PI + angle_prev;
}




float angle_prev_Velocity;				//当前角度(用于速度环)
float vel_angle_prev_Velocity;			//上次角度(用于速度环)
/***********************************************
磁编码器速度计算:(0-∞)
***********************************************/
float getVelocity(float Ts)
{	
	//*********控制周期2ms***********角度差/Ts(rad/s) --> 角度差/(2PI*Ts) (r/s) --> (60*角度差)/(2PI*Ts) (r/min)
 	float _60_Ts_2PI, vel=0.0;

	//计算采样时间
//	angle_prev_ts = SysTick->VAL;
//	if(angle_prev_ts<vel_angle_prev_ts)Ts = (float)(vel_angle_prev_ts - angle_prev_ts)/9*1e-6;
//	else
//		Ts = (float)(0xFFFFFF - angle_prev_ts + vel_angle_prev_ts)/9*1e-6;
//	//快速修复微小溢出
//	if(Ts == 0 || Ts > 0.5) Ts = 1e-3f;

	_60_Ts_2PI=60/(Ts*_2PI);
	
	//记录当前角度
	angle_prev_Velocity = get_Angle();
	//变量存储
	vel = (angle_prev_Velocity - vel_angle_prev_Velocity) * _60_Ts_2PI ;
	vel_angle_prev_Velocity = angle_prev_Velocity;

	return vel;
}


/***********************************************
磁编码器速度低通滤波计算:(0-∞)
***********************************************/
float get_Speed(float Ts)
{
	//速度原始数据采集
	float vel_M0_ori=getVelocity(Ts);
	//原始数据低通滤波
	float vel_M0_flit=LowPass_Filter(DIR*vel_M0_ori);
	return vel_M0_flit;
}
